#include "SurfExtractor.h"
#include "OpenSurf/surf.cpp"
#include "OpenSurf/fasthessian.cpp"
#include "OpenSurf/integral.cpp"
#include "OpenSurf/ipoint.cpp"
#include "OpenSurf/utils.cpp"
#include "OpenSurf/surflib.h"
#include "OpenSurf/kmeans.h"
#include <ctime>

SurfExtractor::SurfExtractor(){
	ROS_INFO("SurfExtractor created");
	upright 		= true;
	octaves 		= 3;
	intervals 		= 2;
	init_sample 	= 3;
	thres			= 0.0001f;
}

SurfExtractor::~SurfExtractor(){}

KeyPointSet * SurfExtractor::getKeyPointSet(IplImage * img, Point *** pointmat, int ** r, int ** g,int ** b){
	IpVec ipts;
	struct timeval start, end;
	gettimeofday(&start, NULL);
	long start_time = start.tv_sec*1000000+start.tv_usec;
	surfDetDes(img, ipts, upright, octaves, intervals, init_sample, thres);
	gettimeofday(&end, NULL);
	long end_time = end.tv_sec*1000000+end.tv_usec;
	ROS_INFO("OpenSURF found: %i interest points",(int)ipts.size());
	ROS_INFO("OpenSURF took: %f seconds",(end_time - start_time)/1000000.0f);
	
	KeyPointSet * keypoints = new KeyPointSet();
	for(int i = 0; i < (int)ipts.size(); i++)
	{
		Ipoint p = ipts.at(i);
		//ROS_INFO("x,y = %i, %i",int(p.x+0.5f),int(p.y+0.5f));
		if(pointmat[int(p.x+0.5f)][int(p.y+0.5f)] != 0){
			keypoints->nr_valid_points++;
		}else{
			keypoints->nr_invalid_points++;
		}
	}
	
	keypoints->stabilety_threshold	= 1.0/thres;
	keypoints->valid_key_points 	= new KeyPoint*[keypoints->nr_valid_points];
	for(int i = 0; i < keypoints->nr_valid_points; i++)
	{
		keypoints->valid_key_points[i] = 0;
	}
	keypoints->invalid_key_points 	= new KeyPoint*[keypoints->nr_invalid_points];
	for(int i = 0; i < keypoints->nr_invalid_points; i++)
	{
		keypoints->invalid_key_points[i] = 0;
	}
	
	int valid_counter 				= 0;
	int invalid_counter 			= 0;
	for(unsigned int i = 0; i < ipts.size(); i++)
	{
		float * desc = new float[64];
		Ipoint p = ipts.at(i);
		
		for(int j = 0; j < 64; j++){
			desc[j] = p.descriptor[j];
		}
		FeatureDescriptor * descriptor = new FeatureDescriptor(desc, 64, p.laplacian, 2);
		
		if(pointmat[int(p.x+0.5f)][int(p.y+0.5f)] != 0){
			keypoints->valid_key_points[valid_counter] 						= new KeyPoint();
			keypoints->valid_key_points[valid_counter]->index_number		= valid_counter;
			keypoints->valid_key_points[valid_counter]->point 				= pointmat[int(p.x+0.5f)][int(p.y+0.5f)];
			keypoints->valid_key_points[valid_counter]->point->keypoint 	= keypoints->valid_key_points[valid_counter];
			keypoints->valid_key_points[valid_counter]->valid				= true;
			keypoints->valid_key_points[valid_counter]->stabilety			= 1.0/thres;
			
			keypoints->valid_key_points[valid_counter]->r					= r[int(p.x+0.5f)][int(p.y+0.5f)];
			keypoints->valid_key_points[valid_counter]->g					= g[int(p.x+0.5f)][int(p.y+0.5f)];
			keypoints->valid_key_points[valid_counter]->b					= b[int(p.x+0.5f)][int(p.y+0.5f)];
			keypoints->valid_key_points[valid_counter]->surf_descriptor		= descriptor;
			valid_counter++;
		}else{
			keypoints->invalid_key_points[invalid_counter] 					= new KeyPoint();
			keypoints->invalid_key_points[invalid_counter]->index_number	= invalid_counter;
			keypoints->invalid_key_points[invalid_counter]->point 			= new Point(0,0,0,int(p.x+0.5f),int(p.y+0.5f));
			keypoints->invalid_key_points[invalid_counter]->point->keypoint = keypoints->invalid_key_points[invalid_counter];
			keypoints->invalid_key_points[invalid_counter]->valid			= false;
			keypoints->invalid_key_points[invalid_counter]->stabilety		= 1.0/thres;
			
			keypoints->invalid_key_points[invalid_counter]->r				= r[int(p.x+0.5f)][int(p.y+0.5f)];
			keypoints->invalid_key_points[invalid_counter]->g				= g[int(p.x+0.5f)][int(p.y+0.5f)];
			keypoints->invalid_key_points[invalid_counter]->b				= b[int(p.x+0.5f)][int(p.y+0.5f)];
			keypoints->invalid_key_points[invalid_counter]->surf_descriptor	= descriptor;
			invalid_counter++;
		}
	}

	return keypoints;
	//return NULL;
}
